//
// Created by juangeng on 26/07/19.
//

#ifndef RL_C1_DRIVER_CHASSISDRIVER_H
#define RL_C1_DRIVER_CHASSISDRIVER_H

#include <ros/ros.h>
//#include <std_msgs/UInt8.h>
//#include <std_msgs/UInt16.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/Twist.h>

#include <string>
#include <sys/msg.h>
#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>

#include "serialport.h"
#include "ip_tcp_connect.h"
#include "nav_msg.h"

typedef uint16_t u16;
typedef uint8_t u8;

#define VEL_MAX   2
#define ANL_MAX   M_PI

//判断异常
#define ERROR_R_F 0x01
#define ERROR_R_M 0x02
#define ERROR_R_B 0x04
#define ERROR_L_F 0x08
#define ERROR_L_M 0x10
#define ERROR_L_B 0x30

#pragma pack(1)

typedef struct
{
    u8 header1;
    u8 header2;
    u8 length;
    u8 sign1;
    u8 sign2;
    u8 stop_mode;
    float liner_speed_x;
    float liner_speed_y;
    float angular_speed;
    u8 check;
} ChassisCtrl;

typedef struct
{
    u8 header1;
    u8 header2;
    u8 length;
    u16 right_front_encodisk;
    u16 right_midlle_encodisk;
    u16 right_back_encodisk;
    u16 left_front_encodisk;
    u16 left_middle_encodisk;
    u16 left_back_encodisk;
    u16 time_stamp_10us;
    u8 right_front_current;
    u8 right_midlle_current;
    u8 right_back_current;
    u8 left_front_current;
    u8 left_midlle_current;
    u8 left_back_current;
    u8 motor_abnormality;
    u8 drive_abnormality;
    u8 encode_disk_abnormality;
    u8 colision_triger;
    short right_front_steer;
    short right_midlle_steer;
    short right_back_steer;
    short left_front_steer;
    short left_midlle_steer;
    short left_back_steer;
    u16 batter_mV;
    u16 charge_mA;
    u8 batter_percent;
    u8 right_front_Hall;
    u8 right_midlle_Hall;
    u8 right_back_Hall;
    u8 left_front_Hall;
    u8 left_midlle_Hall;
    u8 left_back_Hall;
    u8 re1;
    u8 re2;
    u8 re3;
    u8 check;
} FeedBack;

#pragma pack()

class chassisDriver
{
public:
    chassisDriver(){
        port_name_      = std::string("/dev/ttyUSB0");
        baud_rate_      = 115200;
        wheel_diameter_ = 0.15;

        msg_batt_id_  = -1;
        msg_batt_key_ = 1235;
    }
    ~chassisDriver(){
        //关闭线程
        parse_flag_ = false;
        parse_thread_.join();
        ROS_INFO("[chassis driver]: chassis driver exit !");
    }
    void run();

private:
    bool init();
    void twist_callback(const geometry_msgs::Twist::ConstPtr &msg);
//    void pathPointCallback(const wmr_msgs::PathMsg::ConstPtr &msg);
    void send_speed_callback(const ros::TimerEvent &);
    void parse_msg(bool isPrintMsg);

    void exceptionHandle(uint8_t, uint8_t, uint8_t, uint8_t);
    unsigned char form_sum(unsigned char *buf, int len);

private:
    boost::thread parse_thread_;
    boost::mutex twist_mutex_;
    geometry_msgs::Twist current_twist_;

    std::string port_name_;
    int baud_rate_;
    double wheel_diameter_;

    bool parse_flag_;

    FeedBack feed_back_;
    SerialPort *chasis_port_;

    ros::Publisher battery_V_, battery_percent_;

    /****************进程通信-消息队列*********************/
    int msg_batt_id_, msg_batt_key_;
    struct msg_battery_st my_msg_batt_;
    /****************进程通信-消息队列*********************/
};

#endif //RL_C1_DRIVER_CHASSISDRIVER_H
